#include <iostream>
#include <vector>
#include "ros/ros.h"
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>
#include <geometry_msgs/Point32.h>
#include <geometry_msgs/Point.h>
#include <ros/publisher.h>
// 函数声明
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& laser_msg);
bool IsMiddlePointOnClosestLine(const sensor_msgs::LaserScan::ConstPtr& laser_msg, float tolerance);
void publishCroppedCloud(const std::vector<float>& ranges, const sensor_msgs::LaserScan::ConstPtr& scan_msg);

ros::Publisher cloud_pub;
ros::Publisher scan_pub; 

void publishCroppedCloud(const std::vector<float>& ranges, const sensor_msgs::LaserScan::ConstPtr& scan_msg) {
    sensor_msgs::PointCloud cloud;
    cloud.header = scan_msg->header;
    cloud.header.frame_id = "map";  // 确保使用正确的frame_id
    cloud.header.stamp = ros::Time::now();  // 更新时间戳

    // 计算截取部分的起始角度
    float start_angle = scan_msg->angle_min;  
    float angle = start_angle;

    for (size_t i = 0; i < ranges.size(); ++i) {
        float r = ranges[i];
        if (std::isfinite(r) && r > 0.1 && r < 30.0) {  // 添加距离过滤
            geometry_msgs::Point32 pt;
            pt.x = r * std::cos(angle);
            pt.y = r * std::sin(angle);
            pt.z = 0.0;
            cloud.points.push_back(pt);
        }
        angle += scan_msg->angle_increment;
    }

    // 确保点云中有点
    if (!cloud.points.empty()) {
        cloud_pub.publish(cloud);
        ROS_INFO_THROTTLE(1.0, "Publishing cloud with %zu points", cloud.points.empty());
    }
}

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& laser_msg)
{
    scan_pub.publish(*laser_msg);
    // 处理收到的scan消息
    ROS_INFO("Received scan message with %lu ranges", laser_msg->ranges.size());
    // 获取雷达下标210到510的数据-> 机器人后侧
    std::vector<float> ranges(laser_msg->ranges.begin() , laser_msg->ranges.begin() + 333);
    publishCroppedCloud(ranges, laser_msg);
    // 判断雷达测距向量的中间点是否位于最近直线的中点位置
    IsMiddlePointOnClosestLine(laser_msg, 2);
}


geometry_msgs::Point GetMiddlePoint(const sensor_msgs::LaserScan::ConstPtr& laser_msg, int originalMiddleIndex)
{
    float angle = laser_msg->angle_min + originalMiddleIndex * laser_msg->angle_increment;
    float distance = laser_msg->ranges[originalMiddleIndex];
    geometry_msgs::Point middle_point;
    middle_point.y = distance * std::cos(angle);
    middle_point.x = distance * std::sin(angle);
    return middle_point;
}

/**
 * @brief 判断雷达测距向量的中间点是否位于最近直线的中点位置。
 * @param ranges 雷达测距向量。
 * @param tolerance 误差范围。
 * @return 中间点是否位于中点位置。
 */
bool IsMiddlePointOnClosestLine(const sensor_msgs::LaserScan::ConstPtr& laser_msg, float tolerance)
{
    int end_index = 166;
    if (laser_msg->ranges.size() > 335) {
        end_index = 333;
    }
    std::vector<float> ranges(laser_msg->ranges.begin() , laser_msg->ranges.begin() + end_index);
    if (ranges.empty()) {
        return false;
    }

    int n = ranges.size();
    int middleIndex = n / 2;

    // 将误差在2厘米内的点归为一组向量
    std::vector<std::vector<float>> groups;
    std::vector<int> groupStartIndices;
    std::vector<float> currentGroup;
    float lastValue = ranges[0];
    int currentStartIndex = 0;
    int min_group_size = 20; // 最小组大小
    for (int i = 0; i < n; ++i) {
        if (std::abs(ranges[i]) > 1.2)
            continue; // 跳过大于1.5的点
        if (std::abs(ranges[i] - lastValue) <= 0.02) {
            currentGroup.push_back(ranges[i]);
        }
        else { 
            if (!currentGroup.empty() && currentGroup.size() > min_group_size) {
                groups.push_back(currentGroup);
                groupStartIndices.push_back(currentStartIndex);
                currentGroup.clear();
            }
            currentGroup.push_back(ranges[i]);
            currentStartIndex = i;
        }
        lastValue = ranges[i];
    }
    if (!currentGroup.empty() && currentGroup.size() > min_group_size) {
        groups.push_back(currentGroup);
        groupStartIndices.push_back(currentStartIndex);
    }

    ROS_INFO("Total groups found: %zu", groups.size());
    // 找到平均值最小的向量
    std::vector<float> closestLine;
    int closestStartIndex = 0;
    float minAverage = std::numeric_limits<float>::max();
    if (groups.empty()) {
        ROS_WARN("No groups found.");
        return false;
    }

    for (size_t i = 0; i < groups.size(); ++i) {
        const auto& group = groups[i];
        float sum = 0;
        for (float value : group) {
            sum += value;
        }
        // 计算group的首尾差值
        float start_value = group.front();  // 第一个点的距离值
        float end_value = group.back();     // 最后一个点的距离值
        float delta = std::abs(end_value - start_value);  // 首尾距离差的绝对值
        // 如果首尾差值大于0.05，则跳过该组
        if (delta > 0.1) {
            ROS_WARN("Group %zu skipped due to large delta: %f", i, delta);
            continue;
        }

        float average = sum / group.size();
        // ROS_WARN("sum = %f, size = %zu, average = %f, start index = %d", sum, group.size(), average, groupStartIndices[i]);
        if (average < minAverage) {
            minAverage = average;
            closestLine = group;
            closestStartIndex = groupStartIndices[i];
            // 打印
            ROS_WARN("New closest line found: average = %f, size = %zu, start index = %d", average, group.size(), closestStartIndex);
        }
    }

    if (closestLine.empty()) {
        return false;
    }

    // 判断中间点是否位于最近直线的中点位置，误差在一定范围内
    int closestMiddleIndex = closestStartIndex + (closestLine.size() - 1) / 2;
    auto middle_point_ = GetMiddlePoint(laser_msg, closestMiddleIndex);
    ROS_INFO("Middle point: (%f, %f)", middle_point_.x, middle_point_.y);
    // 如果中间点位于最近直线的中点位置
    if (std::abs(middleIndex - closestMiddleIndex) <= tolerance) {
        ROS_ERROR("YES!!");
        // 打印closestLine的大小和中间点的索引
        ROS_ERROR("closestLine size: %lu, middleIndex: %d, closestMiddleIndex: %d", closestLine.size(), middleIndex, closestMiddleIndex);
        return true;
    }
    else {
        ROS_INFO("NO!!");
        //打印closestLine的大小和中间点的索引
        ROS_INFO("closestLine size: %lu, middleIndex: %d, closestMiddleIndex: %d", closestLine.size(), middleIndex, closestMiddleIndex);
        return false;
    }
}

int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "rosbag_find_middle_line");
    ros::NodeHandle nh;
    // 打开rosbag文件
    rosbag::Bag bag;
    bag.open("/home/zhangjj/dm_ws/src/function_test/bag/scan_matched5.bag", rosbag::bagmode::Read);
    cloud_pub = nh.advertise<sensor_msgs::PointCloud>("cropped_cloud", 1);
    scan_pub = nh.advertise<sensor_msgs::LaserScan>("original_scan", 1);

    // 创建一个view，订阅"/scan"话题
    rosbag::View view(bag, rosbag::TopicQuery("/scan"));
    // 遍历view中的所有消息
    for (rosbag::MessageInstance const m : view) {
        // 处理收到的消息
        sensor_msgs::LaserScan::ConstPtr scan_msg = m.instantiate<sensor_msgs::LaserScan>();
        if (scan_msg != nullptr) {
            scanCallback(scan_msg);
        }
        ros::Duration(0.1).sleep();
        // 检查是否有退出信号
        if (ros::isShuttingDown()) {
            ROS_INFO("收到退出信号，程序即将关闭。");
            break; // 退出for循环
        }
    }

    // 关闭rosbag文件
    bag.close();
    return 0;
}